/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.iaroc.irobot.sample;

import com.iaroc.irobot.IRobotCreate;
import com.iaroc.irobot.IRobotCreateConstants;
import com.iaroc.irobot.IRobotCreateEventHandler;
import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.peripheral.LEDColor;

/**
 *
 * @author Joel
 */
public class HomeBaseEventHandler extends IRobotCreateEventHandler 
    {
    public static final int COMMAND_OFFSET = 50;
    
    public static final int STATE_IN_FORCE_FIELD = 0;
    public static final int STATE_IN_GREEN_BUOY = 1;
    public static final int STATE_IN_RED_BUOY = 2;
    public static final int STATE_IN_RED_AND_GREEN = 3;
    public static final int STATE_SEARCH_FOR_SIGNAL = 4;
    
    public static final int LED_LEFT = 0;
    public static final int LED_FRONT_LEFT = 1;
    public static final int LED_FRONT_RIGHT = 6;
    public static final int LED_RIGHT = 7;
    
    protected IRobotCreate create;
    protected EDemoBoard demoBoard;
    protected int state;
    protected int lineNumber;
    protected boolean debugStateMachine;
    protected boolean keepGoing;
    protected boolean inGreenBuoy;
    protected boolean inRedBuoy;
    protected boolean inForceField;
    protected StringBuffer debugRecord;

    public HomeBaseEventHandler(IRobotCreate create, EDemoBoard demoBoard, String 
            str) 
        {
        super();
        this.create = create;
        this.demoBoard = demoBoard;
        lineNumber = 1;
        debugRecord = new StringBuffer(str);
        debugRecord.append("Command Calls:\n\n");
        debugRecord.append("SystemOutput:\n");
        keepGoing = true;
        }
    
    public HomeBaseEventHandler(IRobotCreate create, EDemoBoard demoBoard) 
        {
        this(create, demoBoard, "\nDebug Record: HomeBaseEventHandler\n");
        }
    
    public void start()
        {   
        setDebug(true);
        setDebugStateMachine(true);
        create.sensors(IRobotCreateConstants.SENSORS_GROUP_ID6, null);
        int bit = create.getInfraredByte();
        System.out.print("Byte value is " + bit);
        debugRecord.append(lineNumber++ + " Byte value is " + bit + "\n");
        setState(bit, true);
        create.setVelocity(300);
        //create.goForward();
        }
    
    public void pollSensors() 
        {
        create.sensors(IRobotCreateConstants.SENSORS_GROUP_ID0, this);
        }
    
    public void getInfrared()
        {
        create.sensors(IRobotCreateConstants.SENSORS_INFRARED_BYTE, null);
        }
    
    public void indicateLED()
        {
        System.out.println("g:" + inGreenBuoy + " r:" + inRedBuoy + " f:" + inForceField);
        debugRecord.append("indicateLED(): " +"g:" + inGreenBuoy + " r:" 
                + inRedBuoy + " f:" + inForceField);
        demoBoard.getLEDs()[0].setColor(LEDColor.GREEN);
        demoBoard.getLEDs()[0].setOn(inGreenBuoy);
        demoBoard.getLEDs()[7].setColor(LEDColor.RED);
        demoBoard.getLEDs()[7].setOn(inRedBuoy);
        demoBoard.getLEDs()[1].setColor(LEDColor.WHITE);
        demoBoard.getLEDs()[1].setOn(inForceField);
        }

    public void advanceButtonEvent(boolean oldBoolean, boolean advanceButton) 
        {
        super.advanceButtonEvent(oldBoolean, advanceButton);
        create.stop();
        //Utils.sleep(30000);
        }
    
    public void centerOnSignal(int oldInt, int infraredByte)
        {
        
        }
    
    public void determineOrientation()
        {
        
        }
    
    public void playButtonEvent(boolean oldBoolean, boolean playButton) 
        {
        super.playButtonEvent(oldBoolean, playButton);
        if(playButton)
            System.out.print(debugRecord);
        }

    public void infraredByteEvent(int oldInt, int infraredByte) 
        {
        super.infraredByteEvent(oldInt, infraredByte);
        inForceField = false;
        inGreenBuoy = false;
        inRedBuoy = false;
        switch(infraredByte)
            {
            case IRobotCreateConstants.INFRARED_BYTE_HOME_BASE_FORCE_FIELD:
                inForceField = true;
                break;
            case IRobotCreateConstants.INFRARED_BYTE_HOME_BASE_GREEN_BUOY:
                inGreenBuoy = true;
                break;
            case IRobotCreateConstants.INFRARED_BYTE_HOME_BASE_GREEN_BUOY_FORCE_FIELD:
                inForceField = true;
                inGreenBuoy = true;
                break;
            case IRobotCreateConstants.INFRARED_BYTE_HOME_BASE_RED_BUOY:
                inRedBuoy = true;
                break;
            case IRobotCreateConstants.INFRARED_BYTE_HOME_BASE_RED_BUOY_FORCE_FIELD:
                inForceField = true;
                inRedBuoy = true;
                break;
            case IRobotCreateConstants.INFRARED_BYTE_HOME_BASE_RED_GREEN_BUOY:
                inGreenBuoy = true;
                inRedBuoy = true;
                break;
            case IRobotCreateConstants.INFRARED_BYTE_HOME_BASE_RED_GREEN_BUOY_FORCE_FIELD:
                inForceField = true;
                inGreenBuoy = true;
                inRedBuoy = true;
                break;
            default:
            }
        indicateLED();
//        switch(state)
//            {
//            case STATE_IN_FORCE_FIELD:
//                indicateLED(state, LED_LEFT, LEDColor.WHITE);
//                //create.goForward();
//                create.waitTimeMilliseconds(100);
//                debugRecord.append(lineNumber++ + "State is:" + state + "\n");
//                getInfrared();
//                setState(create.getInfraredByte(), true);
//                debugRecord.insert(COMMAND_OFFSET, "State Reset to: " + state 
//                        + " GOTO: " + lineNumber + "\n");
//                break;
//            case STATE_IN_GREEN_BUOY:
//                indicateLED(state, LED_FRONT_RIGHT, LEDColor.GREEN);
//                
//                //create.drive(300, IRobotCreateConstants.DRIVE_TURN_IN_PLACE_CLOCKWISE);
//                create.waitTimeMilliseconds(100);
//                debugRecord.append(lineNumber++ + "State is:" + state + "\n");
//                getInfrared();
//                setState(create.getInfraredByte(), true);
//                debugRecord.insert(COMMAND_OFFSET, "State Reset to: " + state 
//                        + " GOTO: " + lineNumber + "\n");
//                break;
//            case STATE_IN_RED_BUOY:
//                indicateLED(state, LED_RIGHT, LEDColor.RED);
//                //create.drive(300, IRobotCreateConstants.DRIVE_TURN_IN_PLACE_COUNTER_CLOCKWISE);
//                create.waitTimeMilliseconds(100);
//                debugRecord.append(lineNumber++ + "State is:" + state + "\n");
//                getInfrared();
//                setState(create.getInfraredByte(), true);
//                debugRecord.insert(COMMAND_OFFSET, "State Reset to: " + state 
//                        + " GOTO: " + lineNumber + "\n");
//                break;
//            case STATE_IN_RED_AND_GREEN:
//                indicateLED(state, LED_FRONT_LEFT, LEDColor.TURQUOISE);
//                //create.goForward();
//                create.waitTimeMilliseconds(100);
//                debugRecord.append(lineNumber++ + "State is:" + state + "\n");
//                getInfrared();
//                setState(create.getInfraredByte(), true);
//                debugRecord.insert(COMMAND_OFFSET, "State Reset to: " + state 
//                        + " GOTO: " + lineNumber + "\n");
//                break;
//            case STATE_SEARCH_FOR_SIGNAL:
//                indicateLED(state, 0, LEDColor.BLUE);
//                //create.goForward();
//                create.waitTimeMilliseconds(100);
//                debugRecord.append(lineNumber++ + "State is:" + state + "\n");
//                getInfrared();
//                setState(create.getInfraredByte(), true);
//                debugRecord.insert(COMMAND_OFFSET, "State Reset to: " + state 
//                        + " GOTO: " + lineNumber + "\n");
//                break;
//            default:
//                throw new IllegalStateException("Unknown state " +
//                        "\nState is:" + state);
//                create.drive(300, IRobotCreateConstants.DRIVE_STRAIGHT);
//                create.waitTimeMilliseconds(100);
//            }
      }
    
    public boolean keepGoing() 
        {
        return keepGoing;
        }
    
    public void bumpLeftEvent(boolean oldBoolean, boolean bumpLeft) 
        {
        super.bumpLeftEvent(oldBoolean, bumpLeft);
        keepGoing = false;
        }

    public void bumpRightEvent(boolean oldBoolean, boolean bumpRight) 
        {
        super.bumpRightEvent(oldBoolean, bumpRight);
        keepGoing = false;
        }
    
    protected void setDebugStateMachine(boolean debug) 
        {
        debugStateMachine = debug;
        }
    
    protected void setState(int state, boolean local) 
        {
        if(local)
            {
            switch(state)
                {
                case IRobotCreateConstants.INFRARED_BYTE_HOME_BASE_FORCE_FIELD:
                    state = STATE_IN_FORCE_FIELD;
                    System.out.println("State set to " + state);
                    break;
                    case IRobotCreateConstants.INFRARED_BYTE_HOME_BASE_GREEN_BUOY:
                        state = STATE_IN_GREEN_BUOY;
                    System.out.println("State set to " + state);
                    break;
                case IRobotCreateConstants.INFRARED_BYTE_HOME_BASE_GREEN_BUOY_FORCE_FIELD:
                    state = STATE_IN_GREEN_BUOY;
                    System.out.println("State set to " + state);
                    break;
                case IRobotCreateConstants.INFRARED_BYTE_HOME_BASE_RED_BUOY:
                    state = STATE_IN_RED_BUOY;
                    System.out.println("State set to " + state);
                    break;
                case IRobotCreateConstants.INFRARED_BYTE_HOME_BASE_RED_BUOY_FORCE_FIELD:
                    state = STATE_IN_RED_BUOY;
                    System.out.println("State set to " + state);
                    break;
                case IRobotCreateConstants.INFRARED_BYTE_HOME_BASE_RED_GREEN_BUOY:
                    state = STATE_IN_RED_AND_GREEN;
                    System.out.println("State set to " + state);
                    break;
                case IRobotCreateConstants.INFRARED_BYTE_HOME_BASE_RED_GREEN_BUOY_FORCE_FIELD:
                    state = STATE_IN_RED_AND_GREEN;
                    System.out.println("State set to " + state);
                    break;
                default:
                    state = STATE_SEARCH_FOR_SIGNAL;
                    //throw new IllegalStateException("Unknown state//Set State" +
                      //      "\nState is:" + state);
                }
            }
        if (debug || debugStateMachine) 
            {
            String label;
            switch (state) 
                {
                case STATE_IN_FORCE_FIELD:
                    label = "STATE_IN_FORCE_FIELD";
                    break;
                case STATE_IN_GREEN_BUOY:
                    label = "STATE_IN_GREEN_BUOY";
                    break;
                case STATE_IN_RED_BUOY:
                    label = "STATE_IN_RED_BUOY";
                    break;
                case STATE_IN_RED_AND_GREEN:
                    label = "STATE_IN_RED_AND_GREEN";
                    break;
                case STATE_SEARCH_FOR_SIGNAL:
                    label = "STATE_SEARCH_FOR_SIGNAL";
                    break;
                default:
                    throw new IllegalStateException("Unknown state//Set StateMachine");
                }
            System.out.println(label);
            }
        this.state = state;
        }
}
